/*******************************************************************************
*  file    : dsp_test.cpp
*  created : 06.04.2012
*  author  : Slyshyk Oleksiy (alex312@meta.ua)
*******************************************************************************/

#include "dsp_test.hpp"
#include <algorithm>
#include <cmath>

std::vector<double> meandr_fourier(std::vector<double> & t, int N)
{
    //int A = 1;//амплитуда
    int T = 1;//период

    std::vector<int> nh(N);
    for(size_t i = 1; i <= nh.size(); ++i)
        nh[i-1] = i*2-1;

    std::vector<std::vector<double> > harmonics(N);

    for(size_t i = 0; i < nh.size(); ++i)
        for(size_t j = 0; j < t.size(); ++j)
            harmonics[i].push_back(std::cos(2 * M_PI * nh[i] * t[j]/T ));


    std::vector<double> Am(N);
    for(size_t i = 0; i < Am.size(); ++i)
        Am[i] = i%2 ? -2/M_PI/nh[i]  :   2/M_PI/nh[i]  ;

    std::vector<std::vector<double> > s1(N);
    for(size_t i = 0; i < nh.size(); ++i)
        for(size_t j = 0; j < t.size(); ++j)
            s1[i].push_back( harmonics[i][j] * Am[i] );

    std::vector<double> s2(t.size(),0);
    for(size_t i = 0; i < t.size(); ++i)
        for(size_t j = 0; j < nh.size(); ++j)
            s2[i] += s1[j][i];


    return s2;
}

std::vector<std::complex<double> > dft(const std::vector<double> & data)
{
    std::vector<std::complex<double> > res(data.size(), std::complex<double>(0.0,0.0));

    for(size_t n = 0; n < res.size(); ++n)
        for(size_t k = 0; k < data.size(); ++k)
            res[n] += std::complex<double>(data[k], 0) * std::exp(std::complex<double>(0.0, -2*M_PI*n*k/data.size()));

    return res;
}

std::vector<std::complex<double> > dft(const std::vector<std::complex<double> > & data)
{
    std::vector<std::complex<double> > res(data.size(), std::complex<double>(0.0,0.0));

    for(size_t n = 0; n < res.size(); ++n)
        for(size_t k = 0; k < data.size(); ++k)
            res[n] += data.at(k) * std::exp(std::complex<double>(0.0, -2*M_PI*n*k/data.size()));

    return res;
}

void fft_base(std::vector<std::complex<double> > & a, bool invert)
{
    int n = (int) a.size();
    if (n == 1)  return;

    std::vector<std::complex<double> > a0 (n/2),  a1 (n/2);

    for (int i=0, j=0; i<n; i+=2, ++j)
        {
            a0[j] = a[i];
            a1[j] = a[i+1];
        }
    fft_base (a0, invert);
    fft_base (a1, invert);

    double ang = 2*M_PI/n * (invert ? -1 : 1);
    std::complex<double> w (1);
    std::complex<double> wn (std::cos(ang), std::sin(ang));
    for (int i=0; i<n/2; ++i)
        {
            a[i] = a0[i] + w * a1[i];
            a[i+n/2] = a0[i] - w * a1[i];
            if (invert)
                a[i] /= 2,  a[i+n/2] /= 2;
            w *= wn;
        }
}

std::complex<double> mod_gertzel(boost::circular_buffer<std::complex<double> > & buf,
                                 std::complex<double> sn,
                                 int N,
                                 int k)
{
    static std::complex<double> oldSn;
    std::complex<double> Wn;
    std::complex<double> Sn;

    Wn = std::exp(std::complex<double>(0.0, 2*M_PI*k/N));
    Sn = Wn*(oldSn+sn-buf[0]);

    oldSn = Sn;
    buf.push_back(sn);

    return Sn;
}
